/*
 *Copyright (c) 2024 Black Sesame Technologies
 *
 *Licensed under the Apache License, Version 2.0 (the "License");
 *you may not use this file except in compliance with the License.
 *You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 *Unless required by applicable law or agreed to in writing, software
 *distributed under the License is distributed on an "AS IS" BASIS,
 *WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *See the License for the specific language governing permissions and
 *limitations under the License.
*/

#include "sq_buffer.h"

#include <stdlib.h>

#include "ipc_hw_sem.h"
#include "../can_output.h"

#include "stdlib.h"

#define SHM_SEM_ID (15U)

#define unlikely(x) __builtin_expect((x), 0)

static int is_empty(data_SQ *sq)
{
    return sq->front == sq->rear;
}

int SQ_Buffer_Init(SQ_Buffer *sq, void **buffer, int32_t buffer_size,
                   int32_t buffer_num)
{
    if (unlikely(buffer_num >= SQ_BUFFER_NUM_MAX)) {
        return -1;
    }
    sq->buffer_size = buffer_size;
    sq->buffer_num = buffer_num;
    free_SQ *free_sq = &sq->free_sq;
    for (int i = 0; i < buffer_num; ++i) {
        free_sq->sqe[i].ref_cnt = 0;
        free_sq->sqe[i].buffer = buffer[i];
        free_sq->sqe[i].index = i;
    }
    for (int i = 0; i < SQ_BUFFER_CONSUMER_MAX; ++i) {
        data_SQ *data_sq = &sq->data_sq[i];
        data_sq->front = 0;
        data_sq->rear = 0;
    }
    DcacheMaintRange((uint32)sq, (uint32)sq + sizeof(SQ_Buffer),
                     DCACHE_CLEAN_INVAL_RANGE);

    return 0;
}

SQE *SQ_Buffer_Produce_Get(SQ_Buffer *sq)
{
    DcacheMaintRange((uint32)&sq->free_sq, (uint32)&sq->free_sq + sizeof(free_SQ),
                     DCACHE_CLEAN_INVAL_RANGE);
    free_SQ *free_sq = &sq->free_sq;
    SQE *sqe = NULL;

    for (int i = 0; i < sq->buffer_num; i++) {
        if (free_sq->sqe[i].ref_cnt == 0) {
            sqe = &free_sq->sqe[i];
            break;
        }
    }

    return sqe;
}

int SQ_Buffer_Produce_Put(SQ_Buffer *sq, SQE *sqe, uint16_t consumer_mask)
{
    if (unlikely((consumer_mask & 0xFFFF) == 0)) {
        return -1;
    }

    sqe->ref_cnt = __builtin_popcount(consumer_mask);
    DcacheMaintRange((uint32)&sqe->ref_cnt, (uint32)&sqe->ref_cnt + sizeof(int),
                     DCACHE_CLEAN_INVAL_RANGE);
    for (uint16_t mask = consumer_mask; mask != 0; mask &= mask - 1) {
        int index = __builtin_ctz(mask);
        data_SQ *data_sq = &sq->data_sq[index];
        data_sq->sqe_ptr[data_sq->rear] = sqe->index;
        if (++data_sq->rear == sq->buffer_num + 1) {
            data_sq->rear = 0;
        }
        DcacheMaintRange((uint32)data_sq, (uint32)data_sq + sizeof(data_SQ),
                         DCACHE_CLEAN_INVAL_RANGE);
    }

    return 0;
}

SQE *SQ_Buffer_Consume_Get(SQ_Buffer *sq, uint8_t consumer_id)
{
    if (unlikely(consumer_id >= SQ_BUFFER_CONSUMER_MAX)) {
        return NULL;
    }
    data_SQ *data_sq = &sq->data_sq[consumer_id];
    DcacheMaintRange((uint32)data_sq, (uint32)data_sq + sizeof(data_SQ),
                     DCACHE_CLEAN_INVAL_RANGE);
    if (is_empty(data_sq)) {
        return NULL;
    }
    int index = data_sq->sqe_ptr[data_sq->front];
    SQE *sqe = &sq->free_sq.sqe[index];
    if (++data_sq->front == sq->buffer_num + 1) {
        data_sq->front = 0;
    }
    DcacheMaintRange((uint32)&data_sq->front, (uint32)&data_sq->front + sizeof(int),
                     DCACHE_CLEAN_INVAL_RANGE);
    DcacheMaintRange((uint32)sqe->buffer, (uint32)sqe->buffer + sq->buffer_size,
                     DCACHE_INVAL_RANGE);
    return sqe;
}

int SQ_Buffer_Consume_Put(SQ_Buffer *sq, SQE *sqe)
{
    get_sem_lock(SHM_SEM_ID);
    DcacheMaintRange((uint32)&sqe->ref_cnt, (uint32)&sqe->ref_cnt + sizeof(int),
                     DCACHE_INVAL_RANGE);
    sqe->ref_cnt--;
    DcacheMaintRange((uint32)&sqe->ref_cnt, (uint32)&sqe->ref_cnt + sizeof(int),
                     DCACHE_CLEAN_INVAL_RANGE);
    release_sem_lock(SHM_SEM_ID);

    return 0;
}
